import sys
import math
import time
from PyQt6.QtWidgets import QApplication, QMainWindow
from PyQt6.QtCore import pyqtSlot, QTimer
from app.ui_mainwindow import Ui_MainWindow
from app.serial_service import SerialService
from app.unitree_motor import UnitreeMotorService
from app.net_ft import NetFTService
from app.data_service import DataService
from app.chart_manager import ChartManager

class MainWindow(QMainWindow, Ui_MainWindow):
    def __init__(self):
        super().__init__()
        self.setupUi(self)
        self.init_widgets()

        # 服务实例
        self.serial_service = SerialService(self.on_serial_data)
        self.serial_service.message_signal.connect(self.ui_log)
        self.unitree_motor_service = UnitreeMotorService(self.on_unitree_motor_data)
        self.unitree_motor_service.message_signal.connect(self.ui_log)
        self.data_service = DataService()
        self.data_service.message_signal.connect(self.ui_log)
        self.netft_service = NetFTService(callaback=self.on_netft_data)
        self.netft_service.message_signal.connect(self.ui_log)
        self.chart_manager = ChartManager(self, self.data_service)

        # 启动曲线刷新
        self.chart_timer = QTimer()
        self.chart_timer.timeout.connect(self.chart_manager.update_all_charts)
        self.chart_timer.start(50)  # 每50毫秒刷新一次
        
        # 自动刷新电池电压
        self.battery_timer = QTimer()
        self.battery_timer.timeout.connect(self.update_battery_voltage)
        self.battery_timer.start(1000)  # 每1秒刷新一次
    
    def init_widgets(self):
        # *******串口相关初始化*******
        # 初始化波特率下拉框
        self.BrComboBox.addItems(['115200', '460800', '9600'])
        # 初始化状态标签
        self.ComStateLabel.setText('Disconnected')
        self.Com485StateLabel.setText('Disconnected')
        self.UdpStateLabel.setText('Disconnected')
        self.DataRecordingLabel.setText('Data Recording: Stopped')
        # 串口控件绑定
        self.ScanComPushButton.clicked.connect(self.scan_com_ports)
        self.OpenComPushButton.clicked.connect(self.open_serial)
        self.CloseComPushButton.clicked.connect(self.close_serial)
        self.ComComboBox.currentIndexChanged.connect(self.update_com_info)

        # *******Unitree485控件绑定*******
        self.ScanCom485PushButton.clicked.connect(self.scan_com485_ports)
        self.OpenCom485PushButton.clicked.connect(self.open_com485)
        self.CloseCom485PushButton.clicked.connect(self.close_com485)
        self.Com485ComboBox.currentIndexChanged.connect(self.update_com485_info)
        self.StartServoControlPushButton.clicked.connect(self.start_unitree_motor)
        self.StopServoControlPushButton.clicked.connect(self.stop_unitree_motor)
        self.ServoOffsetPushButton.clicked.connect(self.start_unitree_motor)

        # *******初始化 ATI NET F/T 服务*******
        self.StartUdpPushButton.clicked.connect(self.start_udp)
        self.StopUdpPushButton.clicked.connect(self.stop_udp)

        # *******数据录制相关初始化*******
        self.StartRecordPushButton.clicked.connect(self.start_record)
        self.StopRecordPushButton.clicked.connect(self.stop_record)

        # *******控制指令相关初始化*******
        self.SetHeaterPIDPushButton.clicked.connect(self.set_heater_pid)
        self.SetTemperaturePushButton.clicked.connect(self.set_temperature)
        self.m3508SetPidPushButton.clicked.connect(self.set_motor_pid)
        self.m3508SetRpmPushButton.clicked.connect(self.set_motor_speed)
        self.WeightZeroPushButton.clicked.connect(self.set_weight_offset)
    
    def ui_log(self, message):
        """在UI上显示日志信息"""
        self.SystemStatusTextBrowser.append(message)
        # 日志超过100行,则删除最早的行
        if self.SystemStatusTextBrowser.document().lineCount() > 100:
            cursor = self.SystemStatusTextBrowser.textCursor()
            cursor.select(cursor.SelectionType.Document)
            cursor.removeSelectedText()
            cursor.deletePreviousChar()
        # 滚动到尾部
        cursor = self.SystemStatusTextBrowser.textCursor()
        pos = len(self.SystemStatusTextBrowser.toPlainText())
        cursor.setPosition(pos)
        self.SystemStatusTextBrowser.setTextCursor(cursor)

    @pyqtSlot(list)
    def on_serial_data(self, packets):
        for packet in packets:
            self.data_service.on_data_received(packet)
            # self.ui_log(f'{str(packet)}')

    @pyqtSlot(list)
    def on_unitree_motor_data(self, packets):
        """处理Unitree电机反馈数据"""
        for packet in packets:
            self.data_service.on_data_received(packet)
            # self.ui_log(f'Unitree Motor: {str(packet)}')

    @pyqtSlot(list)
    def on_netft_data(self, packets):
        for packet in packets:
            self.data_service.on_data_received(packet)

    # *********控制指令相关方法*********
    def set_heater_pid(self):
        """设置加热器PID参数"""
        kp = int(self.KpSpinBox.value())
        ki = int(self.KiSpinBox.value())
        kd = int(self.KdSpinBox.value())
        if self.serial_service.write('0xB7', [kp, ki, kd]):
            self.ui_log('Heater PID Set Success')
        else:
            self.ui_log('Heater PID Set Failed')

    def set_temperature(self):
        """设置温度目标值"""
        t0 = int(self.TemperatureGoalCH0SpinBox.value())
        t1 = int(self.TemperatureGoalCH1SpinBox.value())
        t2 = int(self.TemperatureGoalCH2SpinBox.value())
        t3 = int(self.TemperatureGoalCH3SpinBox.value())
        if self.serial_service.write('0xA5', [t0, t1, t2, t3]):
            self.ui_log('Temperature Set Success')
        else:
            self.ui_log('Temperature Set Failed')

    def set_motor_pid(self):
        """设置电机PID参数"""
        kp = int(self.M3508KpSpinBox.value())
        ki = int(self.M3508KiSpinBox.value())
        kd = int(self.M3508KdSpinBox.value())
        if self.serial_service.write('0xB3', [kp, ki, kd]):
            self.ui_log('Motor PID Set Success')
        else:
            self.ui_log('Motor PID Set Failed')

    def set_motor_speed(self):
        """设置电机转速"""
        speed = int(self.M3508RpmSpinBox.value())
        if self.serial_service.write('0xA1', [speed]):
            self.ui_log('Motor Speed Set Success')
        else:
            self.ui_log('Motor Speed Set Failed')

    def set_weight_offset(self):
        """设置重量偏移值"""
        # 获取weight数据
        buff_data = self.data_service.get_data('0x22')
        if buff_data:
            weight = list(buff_data['weight'])
            offset = int(sum(weight[-10:]) / 10.0)
            self.serial_service.write('0xA9', [offset])
            self.ui_log(f'Weight Offset Set Success: {offset}')
        else:
            self.ui_log('Weight Offset Set Failed: No Data Available')

    def start_unitree_motor(self):
        """启动Unitree电机正弦波控制"""
        amp = self.RangeDoubleSpinBox.value() / 180.0 * math.pi
        freq = self.FreqDoubleSpinBox.value()
        angle_offset = self.ServoOffsetDoubleSpinBox.value()
        self.unitree_kp = self.ServoKpDoubleSpinBox.value()
        
        # 记录启动时间，用于计算相对时间
        if not hasattr(self, 'unitree_start_time'):
            self.unitree_start_time = time.time_ns()
        self.unitree_get_rad_pos = lambda t: amp * 6.33 * math.sin(math.pi * 2.0 * freq * t) + angle_offset
        
        if hasattr(self, 'unitree_send_timer') and self.unitree_send_timer is not None:
            self.unitree_send_timer.stop()
        
        self.unitree_send_timer = QTimer()
        self.unitree_send_timer.timeout.connect(self.send_unitree_motor_command)
        self.unitree_send_timer.start(2)

    def send_unitree_motor_command(self):
        """发送Unitree电机控制指令"""
        # 使用相对时间，避免大数值计算问题
        current_time = (time.time_ns() - self.unitree_start_time) / 1_000_000_000
        command_data = {
            'id': 0,
            'Pos': self.unitree_get_rad_pos(current_time),
            'K_P': self.unitree_kp,
            'mode': 1
        }
        self.data_service.on_data_received({
            'identifier': '0x99',
            'data': {
                'Pos': command_data['Pos'],
                'pc_timestamp': current_time
            }
        })
        # 添加异常处理，避免发送失败时导致程序卡死
        try:
            result = self.unitree_motor_service.send_command(command_data)
            if not result:
                self.ui_log("Unitree电机指令发送失败")
        except Exception as e:
            self.ui_log(f"Unitree电机指令发送异常: {str(e)}")
    
    def stop_unitree_motor(self):
        """停止Unitree电机控制"""
        if hasattr(self, 'unitree_send_timer') and self.unitree_send_timer is not None:
            self.unitree_send_timer.stop()
            self.unitree_send_timer = None
            self.ui_log("Unitree电机控制已停止")
    
    def update_battery_voltage(self):
        """更新电池电压显示"""
        voltage_data = self.data_service.get_data('0x11')
        if voltage_data:
            voltage_deque = voltage_data.get('battery_voltage', None)
            if voltage_deque:
                battery_voltage = voltage_deque[-1]
                self.BatteryVoltageLcd.display(battery_voltage)

    # *********串口相关方法*********
    def scan_com_ports(self):
        from serial.tools import list_ports
        self.ComComboBox.clear()
        ports = list_ports.comports()
        for port in ports:
            self.ComComboBox.addItem(port.device, port.description)
    
    def update_com_info(self):
        desc = self.ComComboBox.currentData()
        self.ComInfoLabel.setText(desc if desc else "Unknown")

    def open_serial(self):
        port = self.ComComboBox.currentText()
        baudrate = int(self.BrComboBox.currentText())
        if self.serial_service.open_serial(port, baudrate):
            self.ComStateLabel.setText('Connected')
        else:
            self.ComStateLabel.setText('错误-串口打开失败')

    def close_serial(self):
        self.serial_service.close_serial()
        self.ComStateLabel.setText('Disconnected')
    
    # *********Unitree485相关方法*********
    def scan_com485_ports(self):
        """扫描485串口端口"""
        from serial.tools import list_ports
        self.Com485ComboBox.clear()
        ports = list_ports.comports()
        for port in ports:
            self.Com485ComboBox.addItem(port.device, port.description)
    
    def update_com485_info(self):
        """更新485串口信息显示"""
        desc = self.Com485ComboBox.currentData()
        self.Com485InfoLabel.setText(desc if desc else "Unknown")

    def open_com485(self):
        """打开Unitree485串口"""
        port = self.Com485ComboBox.currentText()
        # Unitree电机通常使用115200波特率，您可以根据需要修改
        baudrate = 4_000_000
        if self.unitree_motor_service.open_serial(port, baudrate):
            self.Com485StateLabel.setText('Connected')
        else:
            self.Com485StateLabel.setText('错误-485串口打开失败')

    def close_com485(self):
        """关闭Unitree485串口"""
        self.unitree_motor_service.close_serial()
        self.Com485StateLabel.setText('Disconnected')
    
    # *********ATI NET F/T 服务相关方法********
    def start_udp(self):
        if self.netft_service.start_streaming():
            self.UdpStateLabel.setText('Connected')
        else:
            self.UdpStateLabel.setText('错误-[NET F/T]启动失败')

    def stop_udp(self):
        self.netft_service.stop_streaming()
        self.UdpStateLabel.setText('Disconnected')

    # *********数据录制相关方法*********
    def start_record(self):
        """开始数据记录"""
        self.data_service.start_record()
        self.DataRecordingLabel.setText('Data Recording: Started')

    def stop_record(self):
        """停止数据记录并保存"""
        file_path = self.data_service.stop_record_and_save()
        if file_path:
            self.DataRecordingLabel.setText(f'Saved: {file_path}')
        else:
            self.DataRecordingLabel.setText('No Data Saved')
